#pragma config(Sensor, in1,    pot,                 sensorPotentiometer)
#pragma config(Sensor, dgtl1,  EncoderR,            sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderL,            sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  TouchRight,          sensorTouch)
#pragma config(Sensor, dgtl6,  TouchLeft,           sensorTouch)
#pragma config(Sensor, dgtl7,  Arm_limit,           sensorTouch)
#pragma config(Sensor, dgtl8,  Arm_closed,          sensorTouch)
#pragma config(Sensor, dgtl11,  Extend_limit,        sensorTouch)
#pragma config(Sensor, dgtl10, Retract_limit,       sensorTouch)
#pragma config(Motor,  port2,           DriveR,        tmotorNormal, openLoop)
#pragma config(Motor,  port3,           DriveL,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port4,           SuckUpper,     tmotorNormal, openLoop)
#pragma config(Motor,  port5,           ArmR,          tmotorNormal, openLoop)
#pragma config(Motor,  port6,           ArmL,          tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           SuckLower,     tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port8,           ExtendR,       tmotorNormal, openLoop)
#pragma config(Motor,  port9,           ExtendL,       tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void stop_all() {
  motor[port1] = 0;
  motor[port2] = 0;
  motor[port3] = 0;
  motor[port4] = 0;
  motor[port5] = 0;
  motor[port6] = 0;
  motor[port7] = 0;
  motor[port8] = 0;
  motor[port9] = 0;
  motor[port10] = 0;
}

int lock(int final_pos) //locks arm to final_pos potentiometer point
{
  int current_pos = SensorValue(in1); //reads potentiometer
  int direction;                      //up or down?
  int arm_diff = abs(current_pos - final_pos);//difference between current and desired points

  if(arm_diff > 100) {  //potentiometer is very precise, so
    if (current_pos > final_pos) {//arm too high
      direction = 1;    //will move down
    }
    else if(current_pos < final_pos) { //arm too low
      direction = -1; //will move up
    }
    motor[port5] = 127 * direction; //sets motors
    motor[port6] = 127 * direction;
    return 0;
  }
  else {  //in hindsight, I don't think this is necessary
    motor[port5] = -10; //no need to move for lock; will continue holding
    motor[port6] = -10;
    return 1;
  }
}

int sucker(int speed) { //positive numbers for out
  motor[port4] = -speed;
  motor[port7] = speed;
  wait1Msec(1000);
  motor[port4] = 0;
  motor[port7] = 0;
  return 1;
}

/* DRIVE FUNCTIONS
 * drive_forward_1msec
 * drive_straight
 * drive_straight_to_touch
 */

 /* drive_forward_msec
  * @purpose: autonomously drive robot forward for provided duration
  * @param speed: speed of motors, -127 to 127; negative values go reverse
  * @param duration: duration in milliseconds
 */

void drive_forward_msec(int speed, int duration) {
  motor[DriveR] = speed;
  motor[DriveL] = speed;
  wait1Msec(duration);
  motor[DriveR] = 0;
  motor[DriveL] = 0;
}
/* drive_straight
  * @purpose: autonomously drive robot forward for provided distance in inches
  * precondition: 4 inch wheels used; if other wheels, change the 4 in the while loop
  * @param speed: speed of motors, -127 to 127; negative values go reverse
  * @param distance: distance in inches
  * TODO: consider adding an acceleration loop
 *///there are 360 ticks in an encoder revolution
void drive_straight(int speed, float distance) {
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;
  while( ( (float) abs(SensorValue[EncoderL])  * 4 / 360 * PI) < distance &&
         ( (float) abs(SensorValue[EncoderR]) * 4 / 360 * PI) < distance) {
    if(abs(SensorValue[EncoderL]) > abs(SensorValue[EncoderR])) {
      motor[DriveR] = -speed;
      motor[DriveL]  = -speed + 1;
    }
    else if(abs(SensorValue[EncoderL]) < abs(SensorValue[EncoderR])) {
      motor[DriveR] = -speed + 1;
      motor[DriveL]  = -speed;
    }
    else {
      motor[DriveR] = -speed;
      motor[DriveL]  = -speed;
    }
  }
  motor[DriveR] = 0;
  motor[DriveL] = 0;
}

/* drive_straight_unlocked
  * @purpose: autonomously drive robot forward for provided distance in inches WHILE allowing
  * precondition: 4 inch wheels used; if other wheels, change the 4 in the while loop
  * @param speed: speed of motors, -127 to 127; negative values go reverse
  * @param distance: distance in inches
  * TODO: consider adding an acceleration loop
 *///there are 360 ticks in an encoder revolution
int drive_straight_unlocked(int speed, float distance) {

  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;
  if( ( (float) abs(SensorValue[EncoderL])  * 2 / 360 * PI) < distance &&
      ( (float) abs(SensorValue[EncoderR]) * 2 / 360 * PI) < distance) {
    if(abs(SensorValue[EncoderL]) > abs(SensorValue[EncoderR])) {

      motor[DriveR] = -speed;
      motor[DriveL] = -speed + 1;
    }
    else if(abs(SensorValue[EncoderL]) < abs(SensorValue[EncoderR])) {

      motor[DriveR] = -speed + 1;
      motor[DriveL]  = -speed;
    }
    else {
      motor[DriveR] = -speed;
      motor[DriveL]  = -speed;
    }
    return 0;
  }
  else
    return 1;
}

/* drive_to_touch
  * @purpose: autonomously drive robot forward until touch sensors are triggered
  * precondition: 2 touch sensors, left and right
  * @param speed: speed of motors, -127 to 127; negative values go reverse
 */
int drive_to_touch(int speed) {
  while((SensorValue[TouchLeft] == 0 || SensorValue[TouchRight] == 0)) {
    if(SensorValue[TouchLeft] == 0)
      motor[DriveL] = -speed;
    else
      motor[DriveL] = 0;
    if(SensorValue[TouchRight] == 0)
      motor[DriveR] = -speed;
    else
      motor[DriveR] = 0;
  }
  return (SensorValue[TouchLeft] == 1 && SensorValue[TouchRight] == 1);
}

/* turn
  * @purpose: autonomously execute a point turn
  * @param speed: speed of motors, 0 to 127; no negative values
  * @param degrees: number of degrees to turn, positive degrees go to left
  * TODO: complete this function
 */
void turn(int speed, int degrees) {
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;
  while(SensorValue[EncoderL] > -degrees || SensorValue[EncoderR] < degrees) {
    motor[DriveL] = speed;
    motor[DriveR] = -speed;
  }
  motor[DriveL] = 0;
  motor[DriveR] = 0;
}

void arm_vertical() {
  while(SensorValue[Arm_limit] == 0) {
    motor[port5] = -127;
    motor[port6] = -127;
  }
  motor[port5] = 0;
  motor[port6] = 0;
  while(SensorValue[Extend_limit] != 1) {
    motor[port8] = 127;
    motor[port9] = 127;
  }
  motor[port8] = 0;
  motor[port9] = 0;
  stop_all();
}

void pull_up() {
  while(SensorValue[Retract_limit] != 0) {
    motor[port8] = -127;
    motor[port9] = -127;
  }
  motor[port8] = 0;
  motor[port9] = 0;
  while(SensorValue[Arm_closed] == 0) {
    motor[port5] = 127;
    motor[port6] = 127;
  }
  motor[port5] = 0;
  motor[port6] = 0;
  stop_all();
}

/* MODE SELECTION FUNCTION
 * mode_select
 * @purpose use a potentiometer to set mode
 * high potentiometer values return 1, low potentiometer values return 0
 */

/*int mode_select() {
  if(SensorValue[Mode] >= 2000)
    return 1;
  else
    return 0;

}*/


/* LINE FOLLOWER FUNCTIONS
 * follow_line_msec
 * follow_line_distance
 */

 /*follow_line_msec
 * @purpose follow a line for provided duration using 3 light sensors
 * @param speed: speed of motors, 0 to 127
 * @param duration: duration of line following in milliseconds
 */
/*void follow_line_msec(int speed, int duration) {
  ClearTimer(T1);
  int threshold = 250;  //guessed threshold; low numbers are lighter
  while(time1[T1] < duration) {
    if(SensorValue[MidLine] < threshold) {  //middle sensor is on line
     motor[DriveL]  = speed;
     motor[DriveR] = speed;
    }
    else if(SensorValue[LeftLine] < threshold) {  //left sensor is on line
      motor[DriveL]  = speed - 1;
      motor[DriveR] = speed;
    }
    else if(SensorValue[RightLine] < threshold) { //right sensor is on line
      motor[DriveL]  = speed;
      motor[DriveR] = speed - 1;
    }
  }
}*/


 /*follow_line_distance
 * @purpose follow a line for provided duration using 3 light sensors
 * precondition: 4-inch wheels
 * @param speed: speed of motors, 0 to 127
 * @param distance: distance to travel in inches
 */
/*void follow_line_distance(int speed, float distance) {
  int threshold = 250;  //guessed threshold; low numbers are lighter
  while(((float)SensorValue[EncoderL]+(float)SensorValue[EncoderR])/2 * 4 / 360 * PI
                                                                      < distance){
    if(SensorValue[MidLine] < threshold) {  //middle sensor is on line
     motor[DriveL]  = speed;
     motor[DriveR] = speed;
    }
    else if(SensorValue[LeftLine] < threshold) {  //left sensor is on line
      motor[DriveL]  = speed - 1;
      motor[DriveR] = speed;
    }
    else if(SensorValue[RightLine] < threshold) { //right sensor is on line
      motor[DriveL]  = speed;
      motor[DriveR] = speed - 1;
    }
  }
}*/
